# 使用OpenCV 寻找 自动查找 和 绘制角
# 尝试校准相机
import numpy as np
import cv2
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import os

# 定义两个空数组来保存这些点 
objpoints = [] # 3D真实的点
imgpoints = [] # 2D的点

# 在数组中创建 6*8 个点
objp = np.zeros((6 * 9, 3), np.float32)
objp[:,:2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)

img = mpimg.imread("camera_cal/calibration4.jpg")
# z转化为灰度图
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

# 找到各个角
ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)

if ret == True:
    imgpoints.append(corners)
    objpoints.append(objp)
    
    # 可视化
    img = cv2.drawChessboardCorners(img, (9, 6), corners, ret)
    plt.imshow(img)
    plt.show()
    r, g, b = cv2.split(img)
    img = cv2.merge((b, g, r))
    cv2.imwrite("output_camera_cal/output_calibration4.jpg", img)